package dijkstra;

import be.kuleuven.cs.som.annotate.Basic;
import be.kuleuven.cs.som.annotate.Immutable;
import be.kuleuven.cs.som.annotate.Raw;
import roborally.factory.PhysicalAmountFactory;
import roborally.itemsonboard.Robot;
import roborally.supporting.PhysicalAmount;
import roborally.supporting.Unit;
import roborally.supporting.Position;

/**
 * To use this class properly make an object and execute the object. Then you can ask the proper values.
 * The shortest path in this case is the path to a destination consuming the least energy. It checks if the position
 * can be reached. The complexity is high, depending the Manhattan distance between the robot and the particular position to reach.
 * 
 * @invar The position to reach must be valid.
 * 		| isValidPositionToReach(positionToReach, robot)
 * 
 * @author Molderez Tom, 2de bach Ingenieurswetenschappen Computerwetenschappen-Elektrotechniek and Reniers Jorn, 2de bach Ingenieurswetenschappen Werktuigkunde-Computerwetenschappen
 * @version 2.0
 */
public class ReachParticularPosition extends Dijkstra {

	private static final double INFINITY = Double.POSITIVE_INFINITY;
	
	/**
	 * Creates a new ReachParticularPosition object with the given parameters.
	 * 
	 * @param robot The robot of this ReachParticularPosition object.
	 * @param positionToReach The position to reach for the given robot.
	 * @pre (repeated)
	 * 		| isValidRobot(robot)
	 * @pre
	 * 		| isValidPositionToReach(positionToReach, robot)
	 * @effect
	 * 		| super(robot)
	 * @post
	 * 		| (this new).getPositionToReach() == positionToReach
	 */
	public ReachParticularPosition(Robot robot, Position positionToReach) {
		super(robot);
		assert isValidPositionToReach(positionToReach, robot);
		this.positionToReach = positionToReach;
	}
	
	/**
	 * Checks if the positionToReach is valid according the given robot.
	 * 
	 * @param positionToReach The position to move to.
	 * @param robot The robot that will be moved.
	 * @pre The robot must be valid
	 * 		| isValidRobot(robot)
	 * @return True if the robot can move to the given position.
	 * 		| result == robot.canHaveAsPosition(positionToReach)
	 */
	public static boolean isValidPositionToReach(Position positionToReach, Robot robot) {
		assert isValidRobot(robot);
		return robot.canHaveAsPosition(positionToReach);
	}
	
	/**
	 * Returns the position to reach.
	 */
	@Basic @Raw @Immutable
	public Position getPositionToReach() {
		return positionToReach;
	}
	
	private final Position positionToReach;
	
	/**
	 * Returns the destination vertex. This vertex contains the position of the target,
	 * the most efficient orientation and the total amount of energy to reach the given
	 * position.
	 * 
	 * If the energy needed to reach the position is PositiveInfinity, the position is not reachable
	 * because of obstacles.
	 * @pre
	 * 		| getState() == DijkstraState.EXECUTED_REACH_POSITION
	 */
	@Basic @Raw
	public Vertex getVertexToReach() {
		assert getState() == DijkstraState.EXECUTED_REACH_POSITION;
		return this.destinationVertex;
	}
	
	private Vertex destinationVertex = null;
	
	/**
	 * By using this method, the shortest path is calculated from the given robot to the destination target. The complexity of this method is high,
	 * so only use it when necessary.
	 * When the state DijkstraState.EXECUTED_REACH_POSITION is true, this method is already been executed and the results can be asked.
	 * 
	 * The method starts with initializing the containers for storage and the creating the initial vertex, of the position of the robot and the destination vertex.
	 * From the priorityQueue with unvisited vertex, we get the vertex that has the shortest distance to the robot. We check if this is the destination,
	 * then the algorithm stops. Otherwise we processes the neighbors of this vertex and add it to the set of visited vertexes. While unvisited contains vertexes,
	 * we repeat.
	 * @pre
	 * 		| (this old).getState() == DijkstraState.NOT_EXECUTED 
	 * @post 
	 * 		| destinationVertex.getEnergyToReach().equals(this.getRobot().getEnergyToReach(destinationVertex.getPosition()))
	 * 		|	|| robot.getEnergyRequiredToReach(destinationVertex.getPosition()).getAmount() == Double.POSITIVE_INFINITY
	 */
	@Override
	public void run() {
		assert this.getState() == DijkstraState.NOT_EXECUTED;
		initialise();
		destinationVertex = new Vertex(positionToReach, null, PhysicalAmountFactory.get().create(INFINITY, Unit.WATTSECOND));
		do {
			Vertex lusVertex = getUnvisited().poll();

			assert !getVisited().contains(lusVertex);

			if (lusVertex.equals(destinationVertex)) {
				this.destinationVertex = lusVertex;
				break;
			}
			
			getVisited().add(lusVertex);
			neighbors(lusVertex);

		} while (!getUnvisited().isEmpty());
		
		setState(DijkstraState.EXECUTED_REACH_POSITION);
	}
	
	/**
	 * Processes the neighbors of this vertex.
	 * 
	 * Asks the position class to return a set with valid positions next to the position of this vertex.
	 * For each position, control if the robot can have that position on the board. If the position is valid,
	 * check is the position is not already visited in the main loop. (Then we already know the minimal distance)
	 * If not, then calculate the energy needed to move from this vertex to the given position. If this is the 
	 * first time we reach that position a new vertex is created and stored. Otherwise the existing vertex is
	 * modified if the energy to reach that vertex is cheaper than the energy of the vertex (the previous 'minimal' energy).
	 * 
	 * @param vertex The vertex to get the neighbors of.
	 */
	private void neighbors(Vertex vertex) {
		for(Position lusPosition : vertex.getPosition().getLegalNeighbors()) {
			if(getRobot().getBoard().canHaveAsItem(new Robot(lusPosition))) {
				Vertex lusVertex = new Vertex(lusPosition, null, PhysicalAmountFactory.get().create(INFINITY, Unit.WATTSECOND));
				if(!getVisited().contains(lusVertex)) {
					int neededTurns = turnsBeforeMovement(vertex, lusVertex);
					lusVertex.setOrientation(vertex.getOrientation().turn(neededTurns));
					PhysicalAmount energy = calculateEnergy(vertex, lusVertex, neededTurns);
					if(!getDistances().containsKey(lusVertex) || energy.compareTo(getDistances().get(lusVertex))==-1) {
						setCheapestPath(lusVertex, energy);
						getCurrentPath().put(vertex, lusVertex);
					}
				}
			}
		}
	}

}
